perm filename MOVE.SAI[SYS,HE] blob
sn#004249 filedate 1972-09-20 generic text, type T, neo UTF8
00100 BEGIN "MOVE"
00200 REQUIRE "PREAMB.SAI[SYS,HE]" SOURCE_FILE;
00300 REQUIRE "VECTOR.SAI[SYS,HE]" SOURCE_FILE;
00400 DEFINE AVT (A, S, CA, SA)="1.0, -CA, SA, A, 1.0, CA, -SA, A, 0.0, SA, CA, S, [3] 0.0, 1.0, 0.0 ";
00500 DEFINE AVP (A, S, CA, SA)="-CA, SA, A, CA, -SA, A";
00600 DEFINE AVS (A, CA, SA, CT, ST)="CT, -CA*ST, SA*ST, A*CT, ST, CA*CT, -SA*CT, A*ST,
00700 0.0, SA, CA, 1.0, [3] 0.0, 1.0, 0.0 ";
00800 DEFINE QT = "0.0, -1.0, 0.0, 0.0, 1.0, [12] 0.0 ";
00900 DEFINE QS = "[11] 0.0, 1.0, [5] 0.0 ";
01000 DEFINE JDEF (M, X, Y, Z, IXX, IYY, IZZ) =".5*(-IXX+IYY+IZZ), 0.0, 0.0, M*X, 0.0,
01100 .5*(IXX-IYY+IZZ), 0.0, M*Y, 0.0, 0.0, .5*(IXX+IYY-IZZ), M*Z, M*X, M*Y, M*Z, M, 0.0 ";
01200 REQUIRE "YELLOW.SAI[SYS,HE]" SOURCE_FILE;
01300 REQUIRE 100 NEW_ITEMS;
01400 REQUIRE 1000 STRING_SPACE;
01500 REQUIRE 256 SYSTEM_PDL;
01600 REQUIRE 200 PNAMES;
01700 DEFINE $="GLOBAL";
01800 BOOLEAN MATCH;
01900 DEFINE ASSIGN="MATCH←FALSE;FOREACH";
02000 DEFINE HOLDS="DO IF MATCH THEN USERERR(0,0,""ASSIGN MULTIPLY DEFINED"")
02100 ELSE MATCH←TRUE;IF ¬MATCH THEN USERERR(0,0,""ASSIGN FAILS"")";
02200 ITEM ORIENTATION,CONTACT;
02300 INTEGER I;
02400 INTEGER N;
02500 SAFE REAL ARRAY ITEMVAR ARRAY ORIENTS[1:30];
02600 INTEGER NO;
02700 STRING FILE;
02800 DEFINE NUM_CELL="100";
02900 SAFE INTEGER ARRAY INDEX[1:NUM_CELL];
03000 SAFE REAL ARRAY RANGE[1:NUM_CELL,0:1];
03100 INTEGER FREE;
03200 INTEGER BREAK,EOF;
03300 SAFE REAL ARRAY TH,DTH,DIR[1:6];
03400 DEFINE MP="MESSAGE";
03500 SAFE REAL ARRAY VA,VO[1:4];
03600 PRELOAD_WITH 0.0, 0.0, 1.0, 1.0;
03700 SAFE REAL ARRAY UZ[1:4];
03800 PRELOAD_WITH [4] 0.0;
03900 SAFE REAL ARRAY ZERO[1:4];
04000 REQUIRE "SAILIB.REL[SYS,HE]" LOAD_MODULE;
04100
00100 STRING SIMPLE PROCEDURE PRINTNAME(ITEMVAR X);
00200 BEGIN INTEGER I;
00300 STRING SI;
00400 PUSH_FORMAT(0,0);
00500 SI←CVIS(X,I);
00600 IF ¬LENGTH(SI) THEN SI ← CVOS(CVN(X));
00700 POP_FORMAT;
00800 RETURN(SI);
00900 END;
01000
01100 INTEGER SIMPLE PROCEDURE GET;
01200 BEGIN INTEGER P;
01300 P←FREE;
01400 FREE←INDEX[FREE];
01500 INDEX[P]←0;
01600 RANGE[P,0]←0.0;
01700 RANGE[P,1]←360;
01800 RETURN(P);
01900 END;
02000 STRING SIMPLE PROCEDURE PRINT(INTEGER I);
02100 BEGIN STRING S;
02200 IF ¬I THEN RETURN("()");
02300 PUSH_FORMAT(7,1);
02400 S←NULL;
02500 WHILE I DO BEGIN S←S&"("&CVF(RANGE[I,0])&CVF(RANGE[I,1])&")";
02600 I←INDEX[I];
02700 END;
02800 POP_FORMAT;
02900 RETURN(S);
03000 END;
03100
03200
03300 SIMPLE PROCEDURE REPLACE(INTEGER L);
03400 IF L THEN BEGIN INTEGER H;
03500 H←L;
03600 WHILE INDEX[L]≠0 DO L←INDEX[L];
03700 INDEX[L]←FREE;
03800 FREE←H;
03900 END;
04000
04100 SIMPLE PROCEDURE RESET_FREE;
04200 BEGIN INTEGER I;
04300 FOR I←1 STEP 1 UNTIL NUM_CELL-1 DO INDEX[I]←I+1;
04400 INDEX[NUM_CELL]←0;
04500 FREE←1;
04600 END;
04700
04800 INTEGER SIMPLE PROCEDURE INTERSECT(INTEGER P1,P2;REAL S);
04900 BEGIN REAL MIN,MAX,R;
05000 INTEGER PR;
05100 IF(RANGE[P1,0]-(R←RANGE[P2,0]+S))*(R-RANGE[P1,1])≥0 THEN MIN←R-S
05200 ELSE IF(RANGE[P2,0]-(R←RANGE[P1,0]-S))*(R-RANGE[P2,1])≥0 THEN MIN←R
05300 ELSE RETURN(0);
05400 IF(RANGE[P1,0]-(R←RANGE[P2,1]+S))*(R-RANGE[P1,1])≥0 THEN MAX←R-S
05500 ELSE IF(RANGE[P2,0]-(R←RANGE[P1,1]-S))*(R-RANGE[P2,1])≥0 THEN MAX←R
05600 ELSE USERERR(0,0,"BAD RANGE ... INTERSECT");
05700 PR←GET;
05800 RANGE[PR,0]←IF MAX=360 THEN MIN-360 ELSE MIN;
05900 RANGE[PR,1]←IF MAX=360 THEN 0.0 ELSE MAX;
06000 RETURN(PR);
06100 END;
06200
06300 INTEGER SIMPLE PROCEDURE INTERSECTION(INTEGER P1,P2);
06400 BEGIN INTEGER PR;
06500 IF(PR←INTERSECT(P1,P2,0)) THEN
06600 BEGIN IF ¬(INDEX[PR]←INTERSECT(P1,P2,360)) THEN INDEX[PR]←INTERSECT(P1,P2,-360) END ELSE
06700 IF ¬(PR←INTERSECT(P1,P2,360)) THEN PR←INTERSECT(P1,P2,-360);
06800 IF PR ∧ INDEX[PR] THEN BEGIN
06900 IF RANGE[PR,1]=RANGE[INDEX[PR],0] THEN
07000 BEGIN RANGE[PR,1]←RANGE[INDEX[PR],1];
07100 REPLACE (INDEX[PR]);
07200 INDEX[PR]←0;
07300 END ELSE IF RANGE[PR,0]=RANGE[INDEX[PR],1] THEN
07400 BEGIN RANGE[PR,0]←RANGE[INDEX[PR],0];
07500 REPLACE (INDEX[PR]);
07600 INDEX[PR]←0;
07700 END;END;
07800 RETURN (PR);
07900 END;
08000
08100 INTEGER SIMPLE PROCEDURE MERGE(INTEGER L1,L2);
08200 BEGIN INTEGER LS,LSA,PL,PR;
08300 PL←0;
08400 LSA←L1;
08500 WHILE L1 DO
08600 BEGIN LS←L2;
08700 WHILE LS DO
08800 BEGIN IF(PR←INTERSECTION(L1,LS)) THEN
08900 BEGIN IF INDEX[PR] THEN
09000 INDEX[INDEX[PR]]←PL ELSE
09100 INDEX[PR]←PL;
09200 PL←PR END;
09300 LS←INDEX[LS] END;
09400 L1←INDEX[L1];
09500 END;
09600 REPLACE(LSA);
09700 REPLACE(L2);
09800 RETURN (PL);
09900 END;
10000
10100 INTEGER SIMPLE PROCEDURE OVERLAP(INTEGER L2,L1;REAL SHIFT);
10200 BEGIN INTEGER LS,PL,PR;
10300 PL←0;
10400 LS←0;
10500 WHILE L2 DO BEGIN IF LS THEN LS←INDEX[LS]←GET ELSE LS←GET;
10600 RANGE[LS,0]←RANGE[L2,0]+SHIFT;
10700 RANGE[LS,1]←RANGE[L2,1]+SHIFT;
10800 L2←INDEX[L2];
10900 END;
11000 L2←LS;
11100 WHILE L1 DO
11200 BEGIN LS←L2;
11300 WHILE LS DO
11400 BEGIN IF(PR←INTERSECTION(L1,LS)) THEN
11500 BEGIN IF INDEX[PR] THEN
11600 INDEX[INDEX[PR]]←PL ELSE
11700 INDEX[PR]←PL;
11800 PL←PR END;
11900 LS←INDEX[LS] END;
12000 L1←INDEX[L1];
12100 END;
12200 REPLACE(L2);
12300 RETURN (PL);
12400 END;
12500
12600 REAL SIMPLE PROCEDURE TAN(REAL R);
12700 RETURN(SIN(R)/COS(R));
12800
12900 SIMPLE PROCEDURE PRINCIPAL(INTEGER P);
13000 BEGIN
13100 WHILE RANGE[P,0]>RANGE[P,1] DO RANGE[P,1]←RANGE[P,1]+360;
13200 WHILE RANGE[P,1]>RANGE[P,0]+360 DO RANGE[P,1]←RANGE[P,1]-360;
13300 WHILE RANGE[P,1]>360 DO
13400 BEGIN RANGE[P,0]←RANGE[P,0]-360;
13500 RANGE[P,1]←RANGE[P,1]-360;
13600 END;
13700 WHILE RANGE[P,0]≤-360 DO
13800 BEGIN RANGE[P,0]←RANGE[P,0]+360;
13900 RANGE[P,1]←RANGE[P,1]+360;
14000 END;
14100 END;
14200
00100 INTEGER SIMPLE PROCEDURE TEN(SAFE REAL ARRAY TRANS);
00200 BEGIN INTEGER P1;
00300 REAL V1,W2,W1,J,T,F,A,M,B1,B2,TFM,C1,C2,SIGN;
00400 SAFE OWN REAL ARRAY P,O,W,VT1,VT2[1:4];
00500 PRELOAD_WITH 0,0,1,1;
00600 SAFE OWN REAL ARRAY K[1:4];
00700 DEFINE L="S3LL+1.5",V2="L↑2+S2↑2";
00800 CVV(P,TRANS,4);
00900 CVV(O,TRANS,2);
01000 V1←SQRT(V2);
01100 DIFFERENCE(W,P,SHOLDER);
01200 REDUCE(W);
01300 W2←DOT(W,W);
01400 W1←SQRT(W2);
01500 IF V1>W1+S6 THEN RETURN(0);
01600 IF W1>V1+S6 THEN RETURN(GET);
01700 J←ACOS((V2+S6↑2-W2)/(2*S6*V1));
01800 T←ASIN(S6*SIN(J)/W1);
01900 F←PI-(J+T);
02000 MOVEV(VT1,O);
02100 VT1[3]←0.0;
02200 UNIT(VT1,VT1);
02300 MOVEV(VT2,W);
02400 VT2[3]←0.0;
02500 UNIT(VT2,VT2);
02600 A←ABS(ASIN(DOT(VT1,VT2)));
02700 M←ACOS(-W[3]/W1);
02800 IF(B1←(SIN(A)*SIN(M)/SIN(F)))<1.0 THEN
02900 B1←ASIN(B1) ELSE RETURN(GET);
03000 B2←PI-B1;
03100 IF A THEN BEGIN
03200 TFM←TAN((F+M)/2);
03300 C1←2*RAD*ATAN2(TFM*COS((A+B1)/2),COS((A-B1)/2));
03400 C2←2*RAD*ATAN2(TFM*COS((A+B2)/2),COS((A-B2)/2));
03500 END ELSE BEGIN
03600 C1←RAD*(M+F);
03700 C2←RAD*(M-F);
03800 END;
03900 P1←GET;
04000 RANGE[P1,0]←90;
04100 RANGE[P1,1]←90;
04200 CROSS(VT1,O,K);
04300 UNIT(VT1,VT1);
04400 SIGN←DOT(VT1,VT2);
04500 IF SIGN <0 THEN
04600 BEGIN RANGE[P1,0]←RANGE[P1,0]+C1;
04700 RANGE[P1,1]←RANGE[P1,1]+C2;
04800 END ELSE
04900 BEGIN RANGE[P1,0]←RANGE[P1,0]-C2;
05000 RANGE[P1,1]←RANGE[P1,1]-C1;
05100 END;
05200 PRINCIPAL(P1);
05300 RETURN (P1);
05400 END;
00100 INTEGER SIMPLE PROCEDURE TABLE_MOV(SAFE REAL ARRAY TRANS);
00200 BEGIN REAL H;
00300 INTEGER P;
00400 H←2.5-TRANS[3,4];
00500 IF H≥2.10 THEN RETURN(0);
00600 P←GET;
00700 IF H>-S6 THEN
00800 BEGIN RANGE[P,0]←RAD*ASIN(H/S6);
00900 RANGE[P,1]←180-RANGE[P,0];
01000 END;
01100 RETURN(P);
01200 END;
01300 INTEGER SIMPLE PROCEDURE POST(SAFE REAL ARRAY TRANS);
01400 BEGIN INTEGER P1;
01500 REAL W2,W1,A,B,S;
01600 SAFE OWN REAL ARRAY P,O,W,VT1,VT2[1:4];
01700 PRELOAD_WITH 0,0,1,1;
01800 SAFE OWN REAL ARRAY K[1:4];
01900 CVV(P,TRANS,4);
02000 DIFFERENCE(W,P,SHOLDER);
02100 REDUCE(W);
02200 IF(W2←(W[1]↑2+W[2]↑2))<S2↑2 THEN RETURN(0);
02300 W1←SQRT(W2);
02400 IF W1>(S2+S6+0.25) THEN RETURN(GET);
02500 CVV(O,TRANS,2);
02600 MOVEV(VT1,O);
02700 VT1[3]←0.0;
02800 UNIT(VT1,VT1);
02900 MOVEV(VT2,W);
03000 VT2[3]←0.0;
03100 UNIT(VT2,VT2);
03200 B←ASIN(S←ABS(DOT(VT1,VT2)));
03300 A←(S*W1/S2);
03400 IF A<1.0 THEN A←ASIN(A)-B ELSE RETURN(GET);
03500 S←SQRT(S2↑2+W2-2*S2*W1*COS(A));
03600 IF (S←S/(S6+0.25))≥1.0 THEN RETURN(GET);
03700 S←RAD*ACOS(S);
03800 P1←GET;
03900 CROSS(VT1,O,K);
04000 UNIT(VT1,VT1);
04100 IF DOT(VT1,VT2)<0 THEN RANGE[P1,0]←RANGE[P1,1]←180 ELSE
04200 RANGE[P1,0]←RANGE[P1,1]←0.0;
04300 RANGE[P1,0]←RANGE[P1,0]+S;
04400 RANGE[P1,1]←RANGE[P1,1]-S;
04500 PRINCIPAL(P1);
04600 RETURN(P1);
04700 END;
00100 BOOLEAN SIMPLE PROCEDURE POSSIBLE(SAFE REAL ARRAY T,J;REAL ROTAT);
00200 BEGIN
00300 EXTERNAL SIMPLE PROCEDURE MOVEV(REAL ARRAY V;REFERENCE REAL R);
00400 EXTERNAL SIMPLE PROCEDURE CROSS(REFERENCE REAL R,A,B);
00500 EXTERNAL SIMPLE PROCEDURE UNIT(REFERENCE REAL R,B);
00600 EXTERNAL SIMPLE PROCEDURE REDUCE(REFERENCE REAL R);
00700 SAFE OWN REAL ARRAY V1,V2,V3[1:4];
00800 INTEGER I;
00900 T[4,1]←T[4,2]←T[4,3]←1.0;
01000 TRANSPOSE(T,T);
01100 T[3,1]←T[3,2]←0.0;
01200 T[3,3]←T[3,4]←1.0;
01300 CROSS(T[1,1],T[2,1],T[3,1]);
01400 UNIT(T[1,1],T[1,1]);
01500 MOVEV(V1,T[1,1]);
01600 MOVEV(V2,T[2,1]);
01700 ROTATE(V3,V1,V2,ROTAT);
01800 FOR I←1 STEP 1 UNTIL 4 DO T[3,I]←V3[I];
01900 CROSS(T[1,1],T[2,1],T[3,1]);
02000 REDUCE(T[1,1]);
02100 REDUCE(T[2,1]);
02200 REDUCE(T[3,1]);
02300 TRANSPOSE(T,T);
02400 T[4,1]←T[4,2]←T[4,3]←0.0;
02500 T[4,4]←1.0;
02600 ARM_SOLVE(T,J,I);
02700 RETURN(I);
02800 END;
02900
03000 INTEGER SIMPLE PROCEDURE LIMIT4(SAFE REAL ARRAY T;INTEGER P2);
03100 BEGIN REAL MID,R;
03200 SAFE OWN REAL ARRAY J[1:6];
03300 REAL UL,LL;
03400 INTEGER P1;
03500 SAFE OWN REAL ARRAY P,O,W,VT1[1:4];
03600 PRELOAD_WITH 0,0,1,1;
03700 SAFE OWN REAL ARRAY K[1:4];
03800 IF (LL←RANGE[P2,0])=0 ∧ (UL←RANGE[P2,1])=360 THEN BEGIN
03900 CVV(P,T,4);
04000 DIFFERENCE(W,P,SHOLDER);
04100 CVV(O,T,2);
04200 CROSS(VT1,O,K);
04300 UL←RAD*ATAN2(-W[3],SQRT(W[1]↑2+W[2]↑2));
04400 IF (R←DOT(VT1,W))<-0.5 THEN UL←180-UL;
04500 IF ABS(R)<0.5 THEN UL←90;
04600 MID←UL-180;
04700 LL←UL-360;
04800 IF POSSIBLE(T,J,MID) THEN RETURN(GET);
04900 IF ¬POSSIBLE(T,J,UL)THEN RETURN (0);
05000 END ELSE BEGIN
05100 IF POSSIBLE(T,J,(MID←(UL+LL)/2))THEN RETURN (GET);
05200 IF ¬POSSIBLE(T,J,UL)THEN RETURN(0);
05300 IF ¬POSSIBLE(T,J,LL)THEN RETURN(0);
05400 END;
05500 R←UL-MID;
05600 WHILE R>5 DO IF ¬POSSIBLE(T,J,(UL←UL-(R←R/2)))THEN UL←UL+R;
05700 R←MID-LL;
05800 WHILE R>5 DO IF ¬POSSIBLE(T,J,(LL←LL+(R←R/2)))THEN LL←LL-R;
05900 P1←GET;
06000 RANGE[P1,1]←LL+5;
06100 RANGE[P1,0]←UL-5;
06200 PRINCIPAL(P1);
06300 RETURN(P1);
06400 END;
06500
06600 INTEGER SIMPLE PROCEDURE ABLE(SAFE REAL ARRAY V,O,T);
06700 BEGIN INTEGER I;
06800 FOR I←1 STEP 1 UNTIL 4 DO BEGIN T[I,2]←O[I];T[I,4]←V[I] END;
06900 IF (I←TEN(T))
07000 THEN IF (I←MERGE(I,LIMIT4(T,I)))
07100 THEN IF (I←MERGE(I,POST(T)))
07200 THEN IF (I←MERGE(I,TABLE_MOV(T)))
07300 THEN RETURN(I) ELSE RETURN(0);
07400 END;
07500
00100 BOOLEAN PROCEDURE CONTAINED (REAL ARRAY C;ITEMVAR F);
00200 BEGIN
00300 LABEL MORE,TRAVEL,NEXTL;
00400 REAL ARRAY ITEMVAR FP,LP,NP;
00500 ITEMVAR L;
00600 SAFE OWN REAL ARRAY PTS[0:40];
00700 BOOLEAN ENCLOSED;
00800 INTEGER I1,I2,N;
00900 SET EDGES;
01000 INTEGER SIMPLE PROCEDURE PRINCIPAL(REAL ARRAY A);
01100 IF ABS(A[1])>ABS(A[2]) THEN RETURN((IF ABS(A[1])>ABS(A[3]) THEN 1 ELSE 3)) ELSE
01200 RETURN((IF ABS(A[2])>ABS(A[3]) THEN 2 ELSE 3));
01300 SIMPLE PROCEDURE INDICES(REFERENCE INTEGER I1,I2;INTEGER N);
01400 BEGIN I1←IF N=1 THEN 2 ELSE 1;
01500 I2←IF N=3 THEN 2 ELSE 3
01600 END;
01700 EXTERNAL BOOLEAN SIMPLE PROCEDURE BOUNDED(REAL X,Y;REAL ARRAY P;VALUE INTEGER N);
01800 REDUCE(C);
01900 COMMENT CHECK IF POINT OF INTERSECTION IS INSIDE BOUNDARY;
02000 ENCLOSED←FALSE;
02100 INDICES(I1,I2,PRINCIPAL(C));
02200 EDGES←($ BOUNDARY⊗F);
02300 MORE: L←LOP(EDGES);
02400 FOREACH FP,LP |$ ENDPT⊗L≡FP ∧ $ ENDPT⊗L≡LP ∧ (LP≠FP) DO DONE;
02500 N←0;
02600 PTS[N]←$ DATUM(LP)[I1];
02700 PTS[N+1]←$ DATUM(LP)[I2];
02800 TRAVEL: FOREACH L,NP | LεEDGES ∧
02900 $ ENDPT⊗L≡LP ∧
03000 $ ENDPT⊗L≡NP ∧
03100 (NP≠LP) DO
03200 BEGIN N←N+2;
03300 PTS[N]←$ DATUM(NP)[I1];
03400 PTS[N+1]←$ DATUM(NP)[I2];
03500 REMOVE L FROM EDGES;
03600 IF NP=FP THEN GO TO NEXTL;
03700 LP←NP;
03800 DONE
03900 END;
04000 GO TO TRAVEL;
04100 NEXTL:IF BOUNDED(C[I1],C[I2],PTS,N+2) THEN ENCLOSED←¬ENCLOSED;
04200 IF EDGES ≠ PHI THEN GO TO MORE;
04300 RETURN(ENCLOSED);
04400 END;
04500
00100 PROCEDURE ORIENT(ITEMVAR BDY);
00200 BEGIN COMMENT FOR BDY THE LIST OF POSSIBLE PICKUP UNIT VECTORS
00300 IS RETURNED IN O. FOR EACH POSSIBLE NO PICKUP POINTS D1 CONTAINS
00400 THE DISTANCE FROM THE CENTER OF MASS TO ONE PICKUP POINT AND D2
00500 CONTAINS THE DISTANCE TO THE OTHER PICKUP POINT. P
00600 CONTAINS THE POINTER TO THE APPROPIATE ORIENTATION;
00700 SAFE REAL ARRAY ITEMVAR A,B,N1,N2,F,V;
00800 ITEMVAR E;
00900 SAFE REAL ARRAY O[1:50,1:4],D1[1:30];
01000 SAFE ITEMVAR ARRAY CP[1:50],TOCP[1:30];
01100 INTEGER NO;
01200 SAFE REAL ARRAY C[1:4],DIST[1:50],TV[1:6];
01300 SAFE REAL ARRAY TVN[1:4];
01400 REAL T,M,D,C2;
01500 INTEGER NPP,N,NPL,NR,I,J,K,II;
01600 INTEGER TPL,NT;
01700 SAFE INTEGER ARRAY POINT[1:50];
01800 SET EDGES;
01900 EXTERNAL REAL SIMPLE PROCEDURE DOT(REFERENCE REAL A,B);
02000 EXTERNAL SIMPLE PROCEDURE REDUCE(REFERENCE REAL R);
02100 EXTERNAL SIMPLE PROCEDURE DIFFERENCE(REFERENCE REAL R,A,B);
02200 EXTERNAL SIMPLE PROCEDURE SCALE(REFERENCE REAL R,A;REAL F);
02300 EXTERNAL SIMPLE PROCEDURE MOVEV(REFERENCE REAL A,B);
02400 EXTERNAL SIMPLE PROCEDURE CROSS(REFERENCE REAL R,A,B);
02500 EXTERNAL SIMPLE PROCEDURE UNIT(REFERENCE REAL A,B);
02600 EXTERNAL REAL SIMPLE PROCEDURE MAGNITUDE(REFERENCE REAL A);
02700 IF TYP_MOVE THEN OUTSTR("ORIENTING "&PRINTNAME(BDY)&'15&'12);
02800 NO←0;
02900 COMMENT WE NOW CALCULATE PICKUP POINTS ON EDGES;
03000 FOREACH E|$ EDGE⊗BDY≡E DO BEGIN
03100 LABEL NEDGE;
03200 FOREACH A,B |$ ENDPT⊗E≡A ∧ $ ENDPT⊗E≡B ∧(A≠B) DO DONE;
03300 DIFFERENCE(TV[1],$ DATUM(A)[1],$ DATUM(B)[1]);
03400 T←DOT($ DATUM(A)[1],TV[1]);
03500 T←T/DOT(TV[1],TV[1]);
03600 IF T≤0.0 ∨ T≥1.0 THEN GO TO NEDGE;
03700 SCALE (TV[1],TV[1],T);
03800 DIFFERENCE(C[1],$ DATUM(A)[1],TV[1]);
03900 COMMENT C IS A VECTOR FROM THE CENTER OF MASS PERPENDICULAR
04000 TO EDGE AT POINT OF CONTACT, NOW WE CHECK TO SEE THAT THIS
04100 IS AN OUTSIDE EDGE;
04200 FOREACH N1,N2|$ BOUNDARY⊗N1≡E ∧ $ BOUNDARY⊗N2≡E ∧ (N1≠N2) DO BEGIN
04300 MOVEV(TVN[1],$ DATUM(N1)[1]);TVN[4]←1.0;
04400 MOVEV(TV[1],$ DATUM(N2)[1]);TV[4]←1.0;
04500 CROSS(TVN[1],TVN[1],TV[1]);
04600 MOVEV(TV[1],$ DATUM(N1)[1]);TV[4]←1.0;
04700 CROSS(TV[1],TV[1],C[1]);
04800 IF DOT(TVN[1],TV[1])≤0.0 THEN GO TO NEDGE;
04900 MOVEV(TV[1],$ DATUM(N2)[1]);TV[4]←1.0;
05000 CROSS(TV[1],TV[1],C[1]);
05100 IF DOT(TVN[1],TV[1])≥0.0 THEN GO TO NEDGE;
05200 NO←NO+1;
05300 MOVEV(O[NO,1],C[1]);
05400 CP[NO]←E;
05500 END;
05600 NEDGE: END;
05700 COMMENT CALCULATE PICKUP POINTS ON FACES;
05800 FOREACH F | $ FACE⊗BDY≡F DO
05900 BEGIN LABEL NFACE;
06000 D←$ DATUM(F)[4];
06100 IF D>0.0 THEN GO TO NFACE;
06200 COMMENT WRONG SIDE OF CENTER OF MASS;
06300 MOVEV(C[1],$ DATUM(F)[1]);
06400 C[4]←-1.0/D;
06500 IF CONTAINED(C,F) THEN
06600 BEGIN NO←NO+1;
06700 MOVEV(O[NO,1],C[1]);
06800 CP[NO]←F;
06900 END;
07000 NPL←NO;
07100 COMMENT NPL POINTS TO THE LAST PLANE;
07200 NFACE: END;
07300 COMMENT CALCULATE PICKUP POINTS ON VERTICES;
07400 EDGES←($ EDGE⊗BDY);
07500 FOREACH V |$ VERTEX⊗BDY≡V DO
07600 BEGIN LABEL NOGOOD;
07700 COMMENT CHECK THAT THIS IS AN OUTSIDE CORNER;
07800 C2←DOT($ DATUM(V)[1],$ DATUM(V)[1]);
07900 FOREACH E,A| Eε EDGES ∧
08000 $ ENDPT⊗E≡A ∧
08100 $ ENDPT⊗E≡V ∧
08200 (A≠V) DO BEGIN
08300 IF DOT($ DATUM(A)[1],$ DATUM(V)[1])>C2 THEN GO TO NOGOOD END;
08400 NO←NO+1;
08500 MOVEV(O[NO,1],$ DATUM(V)[1]);
08600 CP[NO]←V;
08700 NOGOOD: END;
08800 COMMENT NOW NORMALIZE ALL VECTORS IN O AND SET UP A POINTER ARRAY
08900 POINT WHICH POINTS TO VALID VECTORS. AS THE VECTORS ARE NORMALIZED
09000 THEIR DISTANCES ARE STORED IN DIST;
09100 FOR I←1 STEP 1 UNTIL NO DO
09200 BEGIN POINT[I]←I;
09300 COMMENT POINT CONTAINS POINTERS INTO O[I];
09400 DIST[I]←MAGNITUDE(O[I,1]);
09500 COMMENT DIST CONTAINS DISTANCE TO PICKUP POINT;
09600 UNIT(O[I,1],O[I,1]);
09700 END;
09800 NR←NO;
09900 COMMENT WE NOW PAIR OFF ORIENTATION VECTORS;
10000 COMMENT NR IS THE NUMBER OF VECTORS REMAINING;
10100 FOR II←1 STEP 1 WHILE NR>1 ∧ NPL≥POINT[1] DO
10200 BEGIN NPP←0;
10300 TPL←NT←1;
10400 COMMENT NT IS THE POINTER INTO D AN ARRAY OF DISTANCES FOR
10500 VECTORS PARALLEL TO TV. NPP IS FOR KEEPING TRACK OF VECTORS
10600 REMAINING IN O;
10700 MOVEV(TV[1],O[POINT[1],1]);
10800 REDUCE(TV[1]);
10900 D1[1]←DIST[POINT[1]];
11000 TOCP[1]←CP[POINT[1]];
11100 FOR I←2 STEP 1 UNTIL NR DO
11200 BEGIN M←DOT(TV[1],O[POINT[I],1]);
11300 IF ABS(M)>0.985 THEN BEGIN
11400 COMMENT TPL IS THE LAST PLANE CONTACT POINT;
11500 NT←NT+1;
11600 IF POINT[I]≤NPL THEN TPL←NT;
11700 D1[NT]←IF M<0.0 THEN -DIST[POINT[I]] ELSE DIST[POINT[I]];
11800 TOCP[NT]←CP[POINT[I]];
11900 END ELSE
12000 BEGIN NPP←NPP+1;
12100 POINT[NPP]←POINT[I]
12200 END
12300 END;
12400 NR←NPP;
12500 FOR I←1 STEP 1 UNTIL NT-1 DO
12600 FOR J←I+1 STEP 1 UNTIL NT DO
12700 IF D1[I]=D1[J] THEN
12800 BEGIN ARRBLT(D1[J],D1[J+1],NT-J);
12900 ARRBLT(TOCP[J],TOCP[J+1],NT-J);
13000 NT←NT-1;
13100 TPL←IF J≤TPL THEN TPL-1 ELSE TPL
13200 END;
13300 FOR I←1 STEP 1 UNTIL TPL DO
13400 FOR J←I+1 STEP 1 UNTIL NT DO
13500 IF D1[I]*D1[J]<0.0 THEN BEGIN
13600 TV[4]←1.0/D1[I];TV[5]←1.0/D1[J];TV[6]←ABS(D1[I]-D1[J]);
13700 PUSH_FORMAT(6,2);FOR K←1 STEP 1 UNTIL 6 DO IF TYP_MOVE THEN OUTSTR(CVF(TV[K]));IF TYP_MOVE THEN OUTSTR('15&'12);POP_FORMAT;
13800 V←NEW(TV);
13900 MAKE ORIENTATION⊗BDY≡V;
14000 MAKE CONTACT⊗V≡TOCP[I];
14100 MAKE CONTACT⊗V≡TOCP[J];
14200 END;
14300 END;
14400 IF TYP_MOVE THEN OUTSTR(CRLF&CRLF&CRLF&CRLF);
14500 END;
00100 BOOLEAN PROCEDURE ACTUAL
00200 (ITEMVAR PROTO;SAFE REAL ARRAY T;SAFE INTEGER ARRAY RANGES;REFERENCE SET NEIGHBOURS;REFERENCE REAL ARRAY ITEMVAR F);
00300 BEGIN
00400 SAFE OWN REAL ARRAY T1[1:4,1:4];
00500 SAFE OWN REAL ARRAY VT,VT1[1:4];
00600 SET FACES,SUPPORT;
00700 BOOLEAN FOUND_ONE;
00800 SAFE REAL ARRAY ITEMVAR V,N,E;
00900 INTEGER I;
01000 TRANSPOSE(T1,T);
01100 FOR I←1 STEP 1 UNTIL 3 DO T1[4,I]←0.0;
01200 TRANSFORM(VT,T1,UZ);
01300 FACES←NEIGHBOURS←SUPPORT←PHI;
01400 FOREACH F|$ FACE⊗PROTO≡F DO BEGIN
01500 PUT F IN FACES;
01600 MOVEV(VT1,$ DATUM(F));
01700 VT1[4]←1.0;
01800 IF DOT(VT,VT1)<-0.3 ∧ CONTAINED(VT,F) THEN PUT F IN SUPPORT;
01900 END;
02000 ASSIGN F|F ε SUPPORT HOLDS;
02100 IF TYP_MOVE THEN OUTSTR("SUPPORT FACE "&PRINTNAME(F)&CRLF&CRLF);
02200 FOREACH V|$ VERTEX⊗F≡V DO PUT V IN SUPPORT;
02300 FOREACH N,E| $ BOUNDARY⊗F≡E
02400 ∧ $ BOUNDARY⊗N≡E
02500 ∧ (N≠F) DO BEGIN
02600 PUT E IN SUPPORT;
02700 PUT N IN NEIGHBOURS;
02800 END;
02900 FOUND_ONE←FALSE;
03000 FOR I←1 STEP 1 UNTIL NO DO BEGIN
03100 RANGES[I]←RANGES[I]←DATUM(ORIENTS[I])[6]<2.5 ∧ (CONTACT⊗ORIENTS[I])∩SUPPORT=PHI;
03200 FOUND_ONE←FOUND_ONE ∨ RANGES[I];
03300 END;
03400 RETURN(FOUND_ONE);
03500 END;
03600
03700 REAL SIMPLE PROCEDURE GOOD(INTEGER P);
03800 BEGIN REAL R;
03900 INTEGER PT;
04000 FOR R←90,0,270 DO BEGIN
04100 PT←P;
04200 WHILE PT DO IF(RANGE[PT,1]-R)*(R-RANGE[PT,0])≥0 THEN RETURN (R)ELSE PT←INDEX[PT];
04300 END;
04400 RETURN((RANGE[P,1]+RANGE[P,0])/2);
04500 END;
04600
00100 BOOLEAN SIMPLE PROCEDURE COMMON
00200 (SAFE INTEGER ARRAY R1,R2;INTEGER NO;SAFE REAL ARRAY T1,T2,TT1,TT2;REAL LIMIT;REFERENCE REAL MIN_OPENING);
00300 BEGIN INTEGER I,J,K,P;
00400 BOOLEAN FOUND;
00500 SAFE OWN REAL ARRAY AV,OV,VC,VT1,VT2[1:4];
00600 SAFE OWN REAL ARRAY T12[1:4,1:4];
00700 SAFE OWN REAL ARRAY J1,J2[1:6];
00800 REAL DP,RT,R;
00900 FOUND←FALSE;
01000 TRANSPOSE(T12,T1);
01100 FOR I←1 STEP 1 UNTIL 4 DO T12[4,I]←0.0;
01200 TIMES(T12,T2,T12);
01300 T12[4,4]←1.0;
01400 FOR I←1 STEP 1 UNTIL NO DO BEGIN
01500 IF R1[I] ∧ R2[I] ∨ R1[I] ∧ R2[I] THEN BEGIN
01600 MOVEV(AV,DATUM(ORIENTS[I]));
01700 MOVEV(OV,AV);
01800 DATUM(ORIENTS[I])[4]↔DATUM(ORIENTS[I])[5];
01900 MIN_OPENING←DATUM(ORIENTS[I])[6]-0.50;
02000 OV[4]←0.0;
02100 PLUS(AV,AV,DATUM(ORIENTS[I]));
02200 SCALE(AV,AV,0.5);
02300 REDUCE(AV);
02400
02500 BEGIN LABEL L1;
02600 IF ¬R1[I] ∨¬R2[I] THEN GO TO L1;
02700 TRANSFORM(VT1,T1,AV);
02800 IF TYP_MOVE THEN PVECT("P1",VT1);
02900 TRANSFORM(VT2,T1,OV);
03000 IF TYP_MOVE THEN PVECT("O1",VT2);
03100 IF R1[I]=-1 THEN R1[I]←ABLE(VT1,VT2,TT1);
03200 IF TYP_MOVE THEN OUTSTR("R1 "&PRINT(R1[I])&CRLF&CRLF);
03300 IF ¬R1[I] THEN GO TO L1;
03400 FOR K←1 STEP 1 UNTIL 4 DO BEGIN TT1[K,2]←VT2[K];TT1[K,4]←VT1[K] END;
03500 VT2[4]←1.0;
03600 CROSS(VC,VT2,UZ);
03700 TRANSFORM(VT1,T2,AV);
03800 IF TYP_MOVE THEN PVECT("P2",VT1);
03900 TRANSFORM(VT2,T2,OV);
04000 IF TYP_MOVE THEN PVECT("O2",VT2);
04100 IF R2[I]=-1 THEN R2[I]←ABLE(VT1,VT2,TT2);
04200 IF TYP_MOVE THEN OUTSTR("R2 "&PRINT(R2[I])&CRLF&CRLF);
04300 IF ¬R2[I] THEN GO TO L1;
04400 FOR K←1 STEP 1 UNTIL 4 DO BEGIN TT2[K,2]←VT2[K];TT2[K,4]←VT1[K] END;
04500 TRANSFORM(VA,T12,VC);
04600 VT2[4]←1.0;
04700 CROSS(VC,VT2,UZ);
04800 R←ANGLE(VA,VC,VT2);
04900 IF ABS(R/RAD)>LIMIT THEN BEGIN
05000 IF TYP_MOVE THEN OUTSTR("LIMIT EXCEEDED"&CRLF);
05100 RETURN(FALSE);
05200 END;
05300 P←OVERLAP(R1[I],R2[I],R);
05400 IF TYP_MOVE THEN OUTSTR("COMMON RANGE "&PRINT(P)&CRLF&CRLF);
05500 IF P=0 THEN GO TO L1;
05600 RT←GOOD(P);
05700 IF TYP_MOVE THEN BEGIN
05800 PUSH_FORMAT(7,1);
05900 OUTSTR("APPROACH"&CVF(RT)&CRLF&CRLF);
06000 POP_FORMAT;
06100 END;
06200 IF ¬POSSIBLE(TT1,J1,(RT-R)) ∨ ¬POSSIBLE(TT2,J2,RT) THEN
06300 BEGIN
06400 OUTSTR("PREDICTED SOLUTION FAILURE"&CRLF);
06500 GO TO L1;
06600 END;
06700 IF TYP_MOVE THEN PMAT("FIRST MOVE POSITION",TT1);
06800 IF TYP_MOVE THEN PMAT("SECOND MOVE POSITION",TT2);
06900 REPLACE(P);
07000 FOUND←TRUE;
07100 DP←0.0;FOR K←4 STEP 1 UNTIL 6 DO DP←DP+J1[K]*J2[K];
07200 IF DP>0.0 THEN RETURN(TRUE)
07300 ELSE IF TYP_MOVE THEN OUTSTR("TRYING FOR A DIRECT SOLUTION"&CRLF&CRLF);
07400 L1: FOR K←1 STEP 1 UNTIL 3 DO OV[K]←-OV[K];
07500 END;
07600 END;
07700 END;
07800 RETURN(FOUND);
07900 END;
08000
08100 SIMPLE PROCEDURE REVOLVE(SAFE REAL ARRAY P,O; REAL TH);
08200 BEGIN
08300 SAFE OWN REAL ARRAY OP,A,T[1:4];
08400 UNIT(O,O);
08500 SCALE(OP,O,DOT(P,O));
08600 DIFFERENCE(A,P,OP);
08700 CROSS(T,O,A);
08800 SCALE(T,T,SIND(TH));
08900 SCALE(P,A,COSD(TH));
09000 PLUS(P,P,T);
09100 PLUS(P,P,OP);
09200 REDUCE(P);
09300 END;
09400
09500
00100 MP PROCEDURE MOVE_INSTANCE
00200 (REAL ARRAY ITEMVAR BDY;REAL ARRAY NT,IP;REFERENCE INTEGER ARM_PLAN);
00300 BEGIN INTEGER I;
00400 PRELOAD_WITH 0,0,3.0,1.0;
00500 SAFE OWN REAL ARRAY VT[1:4];
00600 SAFE OWN REAL ARRAY TS[1:4,1:4,1:4];
00700 SAFE OWN REAL ARRAY MO[1:2];
00800 SAFE OWN REAL ARRAY T1[1:4,1:4];
00900 PROCEDURE MOVES(SAFE REAL ARRAY ITEMVAR BDY;SAFE REAL ARRAY NT,IP,TS,MO;REFERENCE INTEGER ARM_PLAN);
01000 BEGIN
01100 SAFE REAL ARRAY ITEMVAR O,SS1,SS2,F;
01200 ITEMVAR PROTO;
01300 SET N1,N2;
01400 SAFE OWN INTEGER ARRAY R1,R2,R3[1:20];
01500 SAFE OWN REAL ARRAY VT,VT1,VT2[1:4];
01600 INTEGER I,J;
01700 REAL R,RT,H;
01800 SAFE OWN REAL ARRAY T1,T2,T3,TT1,TT2[1:4,1:4];
01900 LABEL L1,L2,L3,L4;
02000 IF TYP_MOVE THEN OUTSTR("GOING TO MOVE "&PRINTNAME(BDY)&'15&'12);
02100 MATCH←FALSE;
02200 FOREACH PROTO | $ INSTANCE ⊗ PROTO ≡ BDY DO IF MATCH THEN BEGIN ARM_PLAN←-5;RETURN END ELSE MATCH←TRUE;
02300 IF ¬MATCH THEN BEGIN ARM_PLAN←-5;RETURN END;
02400 IF ¬ORIENTATION ⊗ PROTO ≡ ANY THEN BEGIN
02500 IF TYP_MOVE THEN OUTSTR("GOING TO ORIENT "&PRINTNAME(PROTO)&'15&'12);
02600 ORIENT(PROTO);
02700 IF ¬ORIENTATION⊗PROTO ≡ANY THEN BEGIN
02800 IF TYP_MOVE THEN OUTSTR("THERE IS NO WAY THAT THIS BODY MAY BE PICKED UP"&'15&'12);
02900 ARM_PLAN←-1;
03000 RETURN;
03100 END;
03200 END;
03300 NO←0;
03400 FOREACH O| ORIENTATION ⊗ PROTO ≡ O DO ORIENTS[NO←NO+1]←O;
03500 FOR I←1 STEP 1 UNTIL NO-1 DO BEGIN
03600 O←ORIENTS[I];
03700 R←ABS(1/DATUM(ORIENTS[I])[4]-1/DATUM(ORIENTS[I])[5]);
03800 FOR J←I+1 STEP 1 UNTIL NO DO BEGIN
03900 IF (RT←ABS(1/DATUM(ORIENTS[J])[4]-1/DATUM(ORIENTS[J])[5]))<R
04000 THEN BEGIN R←RT; O↔ORIENTS[J] END;
04100 END;
04200 ORIENTS[I]←O;
04300 END;
04400 ARRTRAN(T1,$ DATUM(BDY));
04500 IF TYP_MOVE THEN PMAT("INITIAL POSITION",T1);
04600 ARRTRAN(T2,NT);
04700 IF ¬ACTUAL(PROTO,T1,R1,N1,SS1)THEN BEGIN ARM_PLAN←-2;RETURN END;
04800 IF TYP_MOVE THEN PMAT("FINAL POSITION",T2);
04900 IF ¬ACTUAL(PROTO,T2,R2,N2,SS2)THEN BEGIN ARM_PLAN←-3;RETURN END;
05000 IF COMMON(R1,R2,NO,T1,T2,TT1,TT2,4.0,MO[1]) THEN BEGIN
05100 ARRBLT(TS[1,1,1],TT1[1,1],16);
05200 ARRBLT(TS[2,1,1],TT2[1,1],16);
05300 ARM_PLAN←2;
05400 RETURN;
05500 END;
05600 FOR I←1 STEP 1 UNTIL NO DO IF R1[I] THEN GO TO L1;
05700 ARM_PLAN←-2;
05800 RETURN;
05900 L1: FOR I←1 STEP 1 UNTIL NO DO IF R2[I] THEN GO TO L2;
06000 ARM_PLAN←-3;
06100 RETURN;
06200 L2: IF IP[4]=0.0 THEN BEGIN FOR I←1 STEP 1 UNTIL 3 DO IP[I]←$ DATUM(BDY)[I,4];
06300 IP[4]←1.0;END;
06400 N1←N1∩N2;
06500 N1←N1-{SS1,SS2};
06600 IF TYP_MOVE THEN OUTSTR("NEIGHBOURING FACES"&CVS(LENGTH(N1))&CRLF&CRLF);
06700 MOVEV(VT1,$ DATUM(SS1));
06800 H←IP[3]-ABS(VT1[4]);
06900 VT1[4]←0.0;
07000 TRANSFORM(VT1,T1,VT1);
07100 VT1[4]←1.0;
07200 FOREACH F|F ε N1 DO BEGIN
07300 MOVEV(VT2,$ DATUM(F));
07400 T3[3,4]←H+ABS(VT2[4]);
07500 VT2[4]←0.0;
07600 TRANSFORM(VT2,T1,VT2);
07700 VT2[4]←1.0;
07800 CROSS(VT,VT1,VT2);
07900 UNIT(VT,VT);
08000 R←ANGLE(VT2,VT1,VT);
08100 FOR I←1,2,4 DO T3[I,4]←IP[I];
08200 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
08300 FOR J←1 STEP 1 UNTIL 3 DO VT2[J]←T1[J,I];
08400 VT2[4]←1.0;
08500 REVOLVE(VT2,VT,R);
08600 FOR J←1 STEP 1 UNTIL 3 DO T3[J,I]←VT2[J];
08700 END;
08800 DIFFERENCE(VT2,IP,SHOLDER);
08900 R←ATAN2(VT2[1],VT2[2])-ASIN(S2/SQRT(VT2[1]↑2+VT2[2]↑2));
09000 VT2[1]←SIN(R); VT2[2]←COS(R); VT2[3]←0.0; VT2[4]←1.0;
09100 R←ANGLE(VT,VT2,UZ);
09200 I←(R+45)/90;
09300 R←R-I*90;
09400 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
09500 FOR J←1 STEP 1 UNTIL 3 DO VT2[J]←T3[J,I];
09600 VT2[4]←1.0;
09700 REVOLVE(VT2,UZ,-R);
09800 FOR J←1 STEP 1 UNTIL 3 DO T3[J,I]←VT2[J];
09900 END;
10000 IF TYP_MOVE THEN PMAT("INTERMEDIATE POSITION",T3);
10100 IF ACTUAL(PROTO,T3,R3,N2,SS2) ∧ COMMON(R1,R3,NO,T1,T3,TT1,TT2,3.0,MO[1]) THEN BEGIN
10200 ARRBLT(TS[1,1,1],TT1[1,1],16);
10300 ARRBLT(TS[2,1,1],TT2[1,1],16);
10400 IF COMMON(R3,R2,NO,T3,T2,TT1,TT2,3.0,MO[2]) THEN BEGIN
10500 ARRBLT(TS[3,1,1],TT1[1,1],16);
10600 ARRBLT(TS[4,1,1],TT2[1,1],16);
10700 ARM_PLAN←4;
10800 RETURN;
10900 END;
11000 END;
11100 END;
11200 FOR I←1 STEP 1 UNTIL NO DO IF R1[I] THEN GO TO L3;
11300 ARM_PLAN←-2;
11400 RETURN;
11500 L3: FOR I←1 STEP 1 UNTIL NO DO IF R2[I] THEN GO TO L4;
11600 ARM_PLAN←-3;
11700 RETURN;
11800 L4: ARM_PLAN←-4;
11900 END;
12000
12100 RESET_FREE;
12200 MOVES(BDY,NT,IP,TS,MO,ARM_PLAN);
12300 IF ARM_PLAN≤0 THEN BEGIN
12400 IF TYP_MOVE THEN OUTSTR(CVS(ARM_PLAN)&" SORRY"&'15&'12);
12500 RETURN;
12600 END;
12700 ARRBLT(T1[1,1],TS[1,1,1],16);
12800 ARM_EXECUTE←FALSE;
12900 ISSUE(5,"MOVE","HAND",MESSAGE MOVE_ARM(T1,I));
13000 ISSUE(5,"MOVE","HAND",MESSAGE OPEN_HAND(3.0));
13100 ISSUE(5,"MOVE","HAND",MESSAGE MERGE_ARM);
13200 ISSUE(5,"MOVE","HAND",MESSAGE CLOSE_HAND(MO[1]));
13300 ARRBLT(T1[1,1],TS[2,1,1],16);
13400 T1[3,4]←T1[3,4]+1.0;
13500 ISSUE(5,"MOVE","HAND",MESSAGE MOVE_ARM(T1,I));
13600 ISSUE(5,"MOVE","HAND",MESSAGE PLACE_ARM);
13700 IF ARM_PLAN=4 THEN BEGIN
13800 ARRBLT(T1[1,1],TS[3,1,1],16);
13900 ISSUE(5,"MOVE","HAND",MESSAGE MOVE_ARM(T1,I));
14000 ISSUE(5,"MOVE","HAND",MESSAGE OPEN_HAND(3.0));
14100 ISSUE(5,"MOVE","HAND",MESSAGE MERGE_ARM);
14200 ISSUE(5,"MOVE","HAND",MESSAGE CLOSE_HAND(MO[2]));
14300 ARRBLT(T1[1,1],TS[4,1,1],16);
14400 T1[3,4]←T1[3,4]+1.0;
14500 ISSUE(5,"MOVE","HAND",MESSAGE MOVE_ARM(T1,I));
14600 ISSUE(5,"MOVE","HAND",MESSAGE PLACE_ARM);
14700 END;
14800 END;
14900
00100 FORMAT_POINTER←-1;
00200 PUSH_FORMAT(8,4);
00300 UNDERFLOW(-1);
00400 RESET_FREE;
00500 BREAKSET(1," ,;:","I");
00600 FILE←"ARM";
00700 PUT_DATA(0,0,"MOVE");
00800 OUTSTR(" ***** MOVE INITIALIZED *****"&'15&'12);
00900 YES_MOVE←-1;
01000 WHILE TRUE DO QUEUE('600,GET_ENTRY('120,NULL,"MOVE",NULL));
01100 END;